#ifndef ROBOTCALC_H
#define ROBOTCALC_H
#include "Eigen/Dense"
#include <vector>
#include "global.h"
#include <Eigen/src/Core/Inverse.h>
using namespace  std;
typedef struct{
    float theta;  //θ角
    float a;   //连杆本身长度
    float d;   //连杆和下一个连杆的距离，公共法线的距离
    float alpha;  //连杆本身的角度
    int type;  //类型，1是旋转，2是偏移类型
}TDhStruct;
class RobotCalc
{
public:
    typedef enum{
        TYPE_ROTATE = 0,
        TYPE_MOVE
    }LinkType;
    RobotCalc();
    vector<Matrix44f> ForWard(vector<Matrix14f> theta);  //计算正运动学结果
    vector<Matrix44f> Inverse(vector<Matrix14f> theta);  //计算逆运动学结果
    vector<Matrix44f> Jacobi();  //计算雅克比矩阵
    int AddLink(float,float,float,float,LinkType); //添加机械臂连杆
private:
    float *mTheta; // 关节总数
    int mlinkCnt;
    vector<TDhStruct> mLinks;
};

#endif // ROBOTCALC_H
